/***************************************************************************
 *   Copyright (C) 2010 by Oleg Goncharov  *
 *   $EMAIL$                           *                          
 *                                                                         *
 *   This file is part of ChessVision.                                     *
 *                                                                         *
 *   ChessVision is free software; you can redistribute it and/or modify   *
 *   it under the terms of the GNU General Public License as published by  *
 *   the Free Software Foundation; either version 2 of the License, or     *
 *   (at your option) any later version.                                   *
 *                                                                         *
 *   This program is distributed in the hope that it will be useful,       *
 *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
 *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
 *   GNU General Public License for more details.                          *
 *                                                                         *
 *   You should have received a copy of the GNU General Public License     *
 *   along with this program; if not, write to the                         *
 *   Free Software Foundation, Inc.,                                       *
 *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
 ***************************************************************************/
#ifndef CKATANASENSING_H
#define CKATANASENSING_H

#include <kniBase.h>

namespace Katana {

//TODO Add contact sensors
/** Номера датчиков катаны.
*/
enum Sensors { 
	S_IN_RIGHT_BACK = 0,
	S_IN_RIGHT_FRONT = 1,
	S_IN_BACK_INV = 3,
	S_OUT_RIGHT = 4,
	S_DOWN_RIGHT = 5,
	S_IN_RIGHT_BACK_FORCE = 6,
	S_IN_RIGHT_FRONT_FORCE = 7,
	S_IN_LEFT_BACK = 8,
	S_IN_LEFT_FRONT = 9,
	S_IN_LEFT_BACK_FORCE = 10,
	S_IN_LEFT_FRONT_FORCE = 11,
	S_OUT_LEFT = 12,
	S_DOWN_LEFT = 13,
	S_NSENSORS = 16
};

enum Motors {
  M_BASE = 0,
  M_ARM1 = 1,
  M_ARM2 = 2,
  M_GRPROT = 3,
  M_GRP = 4,
  M_NMOTORS = 5
};
	
/**
	@author Goncharov Oleg <goncharovoi@yandex.ru>
*/
class CKatanaSensing : public CLMBase { 
protected:
	int no_obstacle_value[S_NSENSORS];
	short max_value[S_NSENSORS];
	short min_value[S_NSENSORS];
	bool is_obstacle[S_NSENSORS];
	short fsensors[S_NSENSORS];
	int therehold;
	int alpha;
	const TSctDAT* data;
	
	void Init();
public:
	CKatanaSensing() {}
	
	void create(const char* configurationFile, CCplBase* protocol) {
		CKatana::create(configurationFile, protocol); Init();
	}
	void create(KNI::kmlFactory* infos, CCplBase* protocol) {
		CKatana::create(infos, protocol); Init();
	}
	void create(TKatGNL& gnl, TKatMOT& mot, TKatSCT& sct, TKatEFF& eff, CCplBase* protocol) {
		CKatana::create(gnl, mot, sct, eff, protocol); Init();
	}
	
	/** Настройка фильтра.
	
		filtered_val = (alpha*sens_val + (100-alpha)*previous_filtered_val)/100;
	*/
	void SetFilterAlpha(int _alpha) { alpha = _alpha; }
	
	short GetRawSensorValue(Sensors sensore) { return data->arr[sensore]; } //!< Показания сенсора. 
	const short * GetRawSensors() { return data->arr; } //!< Показания сенсоров.
	short GetSensorValue(Sensors sensore) { return fsensors[sensore]; } //!< Отфильтрованные показания сенсора.
	const short * GetSensors() { return fsensors; } //!< Отфильтрованные показания сенсора.
	
	const short * GetMin() { return min_value; }
	const short * GetMax() { return max_value; }
	
	void SetTherehold(int _therehold) { therehold = _therehold; }
	bool IsObstacle(Sensors sensor) { return is_obstacle[sensor]; }
	short GetNoObstacleValue(Sensors sensor) { return no_obstacle_value[sensor]; }
	
	/** Вычисляет средние значения для отсутствия препятствия.
	*/
	void CalculateNoObstacleValues(int nsamples = 20);
	
	void UpdateSensors(); //!< Обновить показания сенсоров
	
	CMotBase * GetMotor(Motors motor) { return &base->GetMOT()->arr[motor]; }
	bool MotOnTarget(int indx, int mode = 0, int tolerance = 100);
	bool RobotOnTarget(int tolerance = 100);
	
	//TODO чтение и запись в радианах
};

}
#endif
